/**
  ******************************************************************************
  * @file    stepper_motor.c
  * @brief   串口步进电机控制协议驱动
  *          USART6
  ******************************************************************************
  */

#include "stepper_motor.h"
#include "usart.h"


/** @defgroup 内部计算BCC校验位(前九个字节的异或和)函数
  * @{
  */
static uint8_t CalcBCC(const uint8_t *buf, uint8_t len)
{
    uint8_t bcc = 0;
    for (uint8_t i = 0; i < len; i++)
        bcc ^= buf[i];
    return bcc;
}


// 解析电机反馈数据
short Parse_MotorData(StepMotor_ID motor_ID)
{
		rx_index = 0;
    
    // 发送请求命令
		if(motor_ID == 0x01)
		{
			HAL_UART_Transmit(&huart2, motor1_request, sizeof(motor1_request), HAL_MAX_DELAY);
		}
		else if(motor_ID == 0x02)
		{
			HAL_UART_Transmit(&huart2, motor2_request, sizeof(motor2_request), HAL_MAX_DELAY);
		}
		HAL_Delay(10);
    // 启动接收9字节数据
    HAL_UART_Receive_IT(&huart2, rx_buffer, 9);
		HAL_Delay(10);
    if(rx2_complete)
    {
        uint8_t address = rx_buffer[0];           // 电机地址
        uint8_t status = rx_buffer[1];            // 状态位
        uint16_t speed = (rx_buffer[2] << 8) | rx_buffer[3];     // 速度
        uint32_t position = (rx_buffer[4] << 24) | (rx_buffer[5] << 16) | 
                           (rx_buffer[6] << 8) | rx_buffer[7];    // 位置
        uint8_t checksum = rx_buffer[8];          // 校验位

        // 处理数据
        printf("motor_ID: 0x%02X\n", address);
        printf("status: 0x%02X\n", status);
        printf("speed: %d\n", speed);
        printf("position: %d\n", position);
				return status;
    }
		else return 0;
}


/**
  * @brief  按协议封装并发送一帧数据
  * @param  frame 指向用户填充的结构体指针
  * @note   控制须严格按照此协议	
  */
void STEP_USART2_SendFrame(StepMotorFrame_t *frame)
{
    uint8_t tx[11];

    tx[0]  = 0x7B;               /* 帧头 */
		tx[1]  = frame->motor_num;   /* 控制 ID 默认0x01、目前配置俯仰电机ID为0x01，底座电机ID为0x02*/
    tx[2]  = frame->mode;        /* 控制模式0x01 为速度控制模式，0x02 为位置控制模式，0x03为力矩控制模式、0x04 为单圈绝对角度控制模式。*/
    tx[3]  = frame->dir; //0 为逆时针转动，1 为顺时针转动
    tx[4]  = frame->micro;       /* 细分值 */
    tx[5]  = (uint8_t)(frame->posH & 0xFF);
    tx[6]  = (uint8_t)(frame->posL & 0xFF);
    tx[7]  = (uint8_t)(frame->speedH & 0xFF);
    tx[8]  = (uint8_t)(frame->speedL & 0xFF);
    tx[9]  = CalcBCC(tx, 9);     /* BCC 校验 */
    tx[10] = 0x7D;               /* 帧尾 */
	
		HAL_UART_Transmit(&huart2, tx, sizeof(tx), HAL_MAX_DELAY);
}

/**
  * @brief  一步到位的电机控制函数
	* @param  num		 电机ID：默认为0x01，目前配置俯仰电机ID为0x01，底座电机ID为0x02
  * @param  mode   控制模式：STEP_MODE_SPEED / STEP_MODE_POSITION / STEP_MODE_TORQUE / STEP_MODE_ANGLE
  * @param  dir    旋转方向：0-顺时针 1-逆时针
  * @param  micro  细分数：2/4/8/16/32
  * @param  posVal 位置/角度/电流值（单位：角度*10 或 mA） 例如：0x03、0xe8 =0x03e8=1000/10.0=100 度。
  *                 SPEED 模式下填 0
  * @param  speed  转速值（单位：rad/s *10）o h
  *                 非 SPEED 模式填 0
  */
void STEP_MotorCtrl(uint8_t num, StepMode_t mode, uint8_t micro, int16_t posVal,int16_t speed)
{
    StepMotorFrame_t f;
		f.motor_num = num;
    f.mode   		= mode;
    f.micro  		= micro;
		if(posVal <= 0)
		{
			f.dir = 1;
			posVal = -posVal;
		}
		else if(posVal > 0)
		{
			f.dir = 0;
		}
    /* 高低 8 位拆分 */
    f.posH   = (posVal >> 8) & 0xFF;
    f.posL   = posVal & 0xFF;

    f.speedH = (speed >> 8) & 0xFF;
    f.speedL = speed & 0xFF;

    STEP_USART2_SendFrame(&f);
		HAL_Delay(1.1 * 1.939 * posVal  /speed);		//需要延时1111 * theta_rad / V

}

/**
	* @brief  检测云台是否到达指定位置
  */
bool Gimbal_IsAtPosition(void)
{
		uint8_t Base_status = 0;
		uint8_t	Pitch_status = 0;
		Pitch_status = Parse_MotorData(0x01);
		HAL_Delay(50);
		Base_status  = Parse_MotorData(0x02);
		if(Pitch_status != 0x01 || Base_status != 0x01)
		{
			HAL_Delay(50);
		}
		return true;
}

bool Gimbal_IsRotationComplete(void)
{
		if(cam_status == true)
		{
			return true;
		}
		return false;
}

/**
	* @brief  激光使能，拉高为亮
  */
void Laser_Enable(bool enable)
{
	if(enable == true)
	{
		HAL_GPIO_WritePin(Laser_GPIO_Port, Laser_Pin, GPIO_PIN_SET);
	}
	else if(enable == false)
	{
		HAL_GPIO_WritePin(Laser_GPIO_Port, Laser_Pin, GPIO_PIN_RESET);
	}
}
/**
	* @brief  云台复位
  */
void Gimbal_Reset(void)
{
		STEP_MotorCtrl(0x01, STEP_MODE_ANGLE, 0x20, 3200, 100); //电机ID，电机模式，电机方向，电机细分，绝对角度*10，速度*10
		STEP_MotorCtrl(0x02, STEP_MODE_ANGLE, 0x20, 3250, 100); //电机ID，电机模式，电机方向，电机细分，绝对角度*10，速度*10
		x = 160;
		y = 120;
		HAL_Delay(2000);
}

/**
	* @brief  基础题2：小车放在B点处，方向为DA方向
  */
int Gimbal_task1(void)
{
		STEP_MotorCtrl(0x02, STEP_MODE_POSITION, 0x20, 450, 100);
		return 1;
}


/**
	* @brief  基础题3初始：X轴电机旋转360°，让摄像头找到图像
  */
void Gimbal_Rotate360(void)
{
		STEP_MotorCtrl(0x02, STEP_MODE_SPEED, 0x20, 0, 100); //电机ID，电机模式，电机方向，电机细分，绝对角度*10，速度*10
}

/**
	* @brief  设置云台x坐标与y坐标
  */
void Gimbal_SetPosition(int16_t x_pos, int16_t y_pos)
{
		int16_t Gimbal_x_pos = (x_pos - 160);		//摄像头以左上角为原点，中心点为(160,120)
		int16_t Gimbal_y_pos = (120- y_pos) ;	//摄像头向下为正
		STEP_MotorCtrl(0x02, STEP_MODE_POSITION, 0x20, Gimbal_x_pos, 10); //电机ID，电机模式，电机方向，电机细分，绝对角度*10，速度*10
		x = 160;
		if( y >=	450)
		{
			Gimbaly_stop();
		}
		else
		{
			STEP_MotorCtrl(0x01, STEP_MODE_POSITION, 0x20, Gimbal_y_pos, 10); //电机ID，电机模式，电机方向，电机细分，绝对角度*10，速度*10
			y = 120;
		}
		
}

/**
	* @brief  X轴电机旋转angle°
  */
void Gimbal_RotateX(int16_t angle)
{
		STEP_MotorCtrl(0x02, STEP_MODE_ANGLE, 0x20, angle * 10, 100);
}

/**
	* @brief  基础题3初始：X轴电机旋转360°，让摄像头找到图像
  */
void Gimbal_RotateY(int16_t angle)
{
		STEP_MotorCtrl(0x01, STEP_MODE_ANGLE, 0x20, angle * 10, 100);
}

/**
	* @brief  x轴电机停止
  */
void Gimbalx_stop(void)
{
		STEP_MotorCtrl(0x02, STEP_MODE_POSITION, 0x20, 0 ,0);
}

/**
	* @brief  y轴电机停止
  */
void Gimbaly_stop(void)
{
		STEP_MotorCtrl(0x01, STEP_MODE_POSITION, 0x20, 0 ,0);
}
